/*
  Copyright (c) 2017 Qualcomm Technologies, Inc.
  All Rights Reserved.
  Confidential and Proprietary - Qualcomm Technologies, Inc.
*/

#include <AEEStdErr.h>
#include <HAP_perf.h>
#include <HAP_power.h>
#include <HAP_mem.h>
#include <qurt.h>
#include <assert.h>

#include <string.h>
#include <stdlib.h>
#include "tracker_kcf.h"

#include "fastcv_aux.hpp"

#include "utils.hpp"

#define FARF_HIGH 1
#include <HAP_farf.h>

#include <worker_pool.h>


#include "tracker_kcf.h"

#include "TrackerKCF.hpp"


/* Weak reference needed as this API is suported only on Lahaina and later targets */
uint64 HAP_mem_available_stack();
#pragma weak HAP_mem_available_stack
#define FULL_CACHE_OP_THRESHOLD 524288

#define VERSION "0.0.2"

typedef struct {
    void *addr;
    uint32_t len;
    uint32_t offset;
    int fd;
} mmap_buffer_t;

typedef struct {
    mmap_buffer_t buffer;
    int width;
    int height;
    TrackerKCF *tracker; 
    int frame_cnt;
    // worker_pool_context_t worker_pool;
} tracker_kcf_context_t;

static AEEResult do_mmap_buff_in(mmap_buffer_t *buffer)
{
    /* Invalidate caches for the buffer. If the buffer is larger than a threshold,
       we'll simply invalidate the entire cache rather than walk through the buffer
       by virtual address. */
    if ( buffer->len < FULL_CACHE_OP_THRESHOLD ) {
        FARF(HIGH, "Invalidate buffer at 0x%08x, len %u\n", buffer->addr, buffer->len);
        qurt_mem_cache_clean((qurt_addr_t)buffer->addr, buffer->len, QURT_MEM_CACHE_INVALIDATE, QURT_MEM_DCACHE);
    } else {
        FARF(HIGH, "Invalidate all. Buffer at 0x%08x, len %u\n", buffer->addr, buffer->len);
        qurt_mem_cache_clean((qurt_addr_t)buffer->addr, buffer->len, QURT_MEM_CACHE_FLUSH_INVALIDATE_ALL, QURT_MEM_DCACHE);
    }
    return 0;
}

static AEEResult do_mmap_buff_out(mmap_buffer_t *buffer)
{
    if (buffer->addr == NULL) {
        return 0;
    }
    /* Flush caches for the buffer. If the buffer is larger than a threshold,
       we'll simply flush the entire cache rather than walk through the buffer
       by virtual address. */
    if ( buffer->len < FULL_CACHE_OP_THRESHOLD ) {
        FARF(HIGH, "Flush buffer at 0x%08x, len %u\n", buffer->addr, buffer->len);
        qurt_mem_cache_clean((qurt_addr_t)buffer->addr, buffer->len, QURT_MEM_CACHE_FLUSH, QURT_MEM_DCACHE);
    } else {
        FARF(HIGH, "Flush all. Buffer at 0x%08x, len %u\n", buffer->addr, buffer->len);
        qurt_mem_cache_clean((qurt_addr_t)buffer->addr, buffer->len, QURT_MEM_CACHE_FLUSH_ALL, QURT_MEM_DCACHE);
    }
    return 0;
}

static AEEResult do_mmap_unmap(mmap_buffer_t *buffer)
{

    int retval = -1;
    if (buffer->addr == NULL) {
        return 0;
    }
    void *mmap_address = buffer->addr;
    /*
     * HAP_mmap_put : Decrement the reference count of the file descriptor
     */
    FARF(HIGH,"\nHAP_mmap_put:\n");

    retval = HAP_mmap_put(buffer->fd);

    if(retval!=AEE_SUCCESS)
    {
        FARF(ERROR,"Error Code -1x%x returned from function : HAP_mmap_put\n", retval);
        goto bail;
    }

    FARF(HIGH,"Decremented the reference count of the buffer with buffer_fd = %d\n", buffer->fd);

    /*
     * HAP_munmap : To decrease the reference count and unmap the buffer from memory at the given address if the reference count goes to -1
     */
    FARF(HIGH,"\nHAP_munmap:\n");

    retval = HAP_munmap(mmap_address, buffer->len);

    if(retval!=AEE_SUCCESS)
    {
        FARF(ERROR,"Error Code -1x%x returned from function : HAP_munmap\n", retval);
        goto bail;
    }

    FARF(HIGH,"Unmapped the buffer starting at mmap_address = %p with length buffer_len = %lu\n", mmap_address, buffer->len);
    buffer->addr = NULL;
    buffer->len = 0;
    buffer->offset = 0;
    buffer->fd = -1;
bail:
    if(retval==AEE_SUCCESS)
    {
        FARF(HIGH,"\nmem_dmahandle PASSED\n");
    }
    else
    {
        FARF(HIGH,"\nmem_dmahandle FAILED\n");
    }

    return retval;
}

AEEResult tracker_kcf_open(const char *uri, remote_handle64 *h)
{
    FARF(HIGH, "tracker_kcf_open");
    FARF(HIGH, "tracker_kcf version: %s-%s-%s", VERSION, __DATE__, __TIME__);

    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*)malloc(sizeof(tracker_kcf_context_t));
    if(ctx == nullptr)
    {
        FARF(ERROR, "Failed to allocate memory for the tracker_kcf context");
        return AEE_ENOMEMORY;
    }
    memset(ctx, 0, sizeof(tracker_kcf_context_t));

    FcvContext::getContext()->init();

    FARF(HIGH,"num workers: %d", num_workers);

    // BlobDetectionParams params;
    // ctx->detector.reserve(num_workers);
    // for (int i = 0; i < num_workers; i++) {
    //     auto ptr = new DspBlobDetector(params);
    //     assert(ptr != NULL);
    //     ctx->detector.push_back(ptr); 
    // }

    // init worker pool
    // int res = worker_pool_init(&ctx->worker_pool);
    // if(res != AEE_SUCCESS)
    // {
    //     FARF(ERROR, "Unable to create worker pool");
    //     return AEE_EBADHANDLE;
    // }

    *h = (remote_handle64) ctx;

    FARF(HIGH, "---------create_tracker_kcf success-------");

    return AEE_SUCCESS;
}


AEEResult tracker_kcf_close(remote_handle64 h)
{
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) h;

    if(ctx == NULL)
    {
        FARF(ERROR, "NULL handle received in tracker_kcf_close()");
        return AEE_SUCCESS;
    }
    do_mmap_buff_out(&ctx->buffer);
    do_mmap_unmap(&ctx->buffer);
    // if(ctx->worker_pool != NULL)
    // {
    //     //deinit worker pool
    //     worker_pool_deinit(&ctx->worker_pool);
    // }

    if (ctx->tracker) {
        delete ctx->tracker;
    }

    free(ctx);

    FcvContext::getContext()->deinit();

    FARF(HIGH, "tracker_kcf_close");
    return AEE_SUCCESS;
}

AEEResult tracker_kcf_init(remote_handle64 handle, int32_t img_width, int32_t img_height, int32_t patch_size, boolean multiscale)
{
    FARF(HIGH, "tracker_kcf_init");
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) handle;
    ctx->width = img_width;
    ctx->height = img_height;
    ctx->tracker = new TrackerKCF(patch_size, multiscale);
    if (ctx->tracker == NULL) {
        FARF(ERROR, "create TrackerKCF failed: patch_size=%d, multiscale:%d", patch_size, multiscale);
        return AEE_ENOMEMORY;
    }

    FARF(HIGH, "tracker_kcf_init success");
    return AEE_SUCCESS;
}

AEEResult tracker_kcf_mem_dmahandle(remote_handle64 handle, int buffer_fd, uint32 buffer_offset, uint32 buffer_len)
{

    int retval = 0;

    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) handle;
    /*
     * HAP_mmap : To allocate memory for and map the buffer with the given file descriptor to Memory and initialize the reference count to 1
     *            Increment the reference count if already mapped
     */
    FARF(HIGH,"\nHAP_mmap:\n");

    void *mmap_address;
    int prot = HAP_PROT_READ | HAP_PROT_WRITE;
    int flags = 0, avail_stack = 0;

    mmap_address = HAP_mmap(NULL, buffer_len, prot, flags, buffer_fd, buffer_offset);


    if(mmap_address==(void*)0xFFFFFFFF)
    {
        FARF(ERROR,"Failed to allocate memory and map the buffer using : HAP_mmap\n");
        retval = -1;
        goto bail;
    }

    FARF(HIGH,"buffer_len = %lu\nprot = %d\nflags = %d\nbuffer_fd=%d\nbuffer_offset=%lu\nmmap_address = %p\n",
                 buffer_len, prot, flags, buffer_fd, buffer_offset, mmap_address);


    /*
     * HAP_mmap_get : To get the address associated with the buffer and increment its reference count
     */
    FARF(HIGH,"\nHAP_mmap_get:\n");

    uint64 phys_address;

    retval = HAP_mmap_get(buffer_fd, NULL, &phys_address);

    if(retval!=AEE_SUCCESS)
    {
        FARF(ERROR,"Error Code 0x%x returned from function : HAP_mmap_get\n", retval);
        goto bail;
    }

    FARF(HIGH,"Incremented the reference count of buffer with buffer_fd = %d and physical address = %llu\n", buffer_fd, phys_address);


    /* NOTE:
     *  If the buffer is used to share data with other cores, cache maintenance operations must be performed manually.
     *  This is done on the DSP using qurt_mem_cache_clean() API to invalidate or flush the cache.
     *  e.g.:
     *      1)Flush the caches for the buffer :
     *          retval = qurt_mem_cache_clean((qurt_addr_t)mmap_address, (qurt_size_t)buffer_len, QURT_MEM_CACHE_FLUSH, QURT_MEM_DCACHE);
     *
     *      2)Invalidate the caches for the buffer :
     *          retval = qurt_mem_cache_clean((qurt_addr_t)mmap_address, (qurt_size_t)buffer_len, QURT_MEM_CACHE_INVALIDATE, QURT_MEM_DCACHE);
     */


    /*
     * HAP_mem_available_stack : Get the stack size (in bytes) available for current thread
     */
    if (HAP_mem_available_stack == NULL )
    {
        FARF(ERROR,"%s: HAP_mem_available_stack() not supported \n", __func__);
    }
    else
    {
        avail_stack = HAP_mem_available_stack();
        if ((avail_stack == AEE_EINVALIDTHREAD) || (avail_stack == AEE_ERESOURCENOTFOUND))
        {
            FARF(ERROR,"Error Code 0x%x returned from function : HAP_mem_available_stack\n", avail_stack);
            goto bail;
        }
        else
        {
            FARF(HIGH,"Available stack size is %d bytes \n", avail_stack);
        }
    }
    ctx->buffer.addr= mmap_address;
    ctx->buffer.fd = buffer_fd;
    ctx->buffer.len = buffer_len;
    ctx->buffer.offset = buffer_offset;
bail:

    if(retval==AEE_SUCCESS)
    {
        FARF(HIGH,"\nmem_dmahandle PASSED\n");
    }
    else
    {
        FARF(HIGH,"\nmem_dmahandle FAILED\n");
    }

    return retval;
}


extern bool gDbg;
AEEResult tracker_kcf_update(remote_handle64 handle,
                            const roi_t *roi, 
                            int buffer_fd,
                            float train_interp_factor,
                            int32_t *dsp_usec)
{
    TicToc tic;
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) handle;
    do_mmap_buff_in(&ctx->buffer);
    return tracker_kcf_update2(handle, roi, buffer_fd, train_interp_factor, dsp_usec);
}

AEEResult tracker_kcf_update2(remote_handle64 handle,
                            const roi_t *roi, 
                            int buffer_fd,
                            float train_interp_factor,
                            int32_t *dsp_usec)
{
    TicToc tic;
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) handle;
    // if (ctx->frame_cnt > 50 && ctx->frame_cnt < 53) {
    //     gDbg = true;
    // } else {
    //     gDbg = false;
    // }
    FARF(HIGH, "tracker_kcf_update[%d]: [%d, %d, %d, %d], %f\n", ctx->frame_cnt, roi->x, roi->y, roi->width, roi->height, train_interp_factor);
    if (ctx->tracker) {
        CvMatU8 img((uint8_t*)(ctx->buffer.addr), ctx->height, ctx->width);
        CvRect rect;
        rect.x = roi->x;
        rect.y = roi->y;
        rect.width = roi->width;
        rect.height = roi->height;
        ctx->tracker->update(rect, img, train_interp_factor);
    }
    *dsp_usec = (static_cast<int32_t>(tic.toc()));
    FARF(HIGH, "tracker_kcf_update: %lluus\n", tic.toc());
    return 0; 
}

AEEResult tracker_kcf_detect(remote_handle64 handle, 
                            const roi_t *in_roi,
                            int buffer_fd,
                            roi_t *out_roi,
                            float *peak_value,
                            int32_t *dsp_usec) 
{
    TicToc tic;
    AEEResult ret = 0;
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) handle;
    FARF(HIGH, "tracker_kcf_detect[%d], in_roi:[%d,%d,%d,%d]\n", ctx->frame_cnt, in_roi->x, in_roi->y, in_roi->width, in_roi->height);
    if (ctx->tracker) {
        do_mmap_buff_in(&ctx->buffer);
        CvMatU8 img((uint8_t*)(ctx->buffer.addr), ctx->height, ctx->width);
        CvRect rect;
        rect.x = in_roi->x;
        rect.y = in_roi->y;
        rect.width = in_roi->width;
        rect.height = in_roi->height;
        float peak_value_;
        CvRect out_rect = ctx->tracker->detect(rect, img, peak_value_); 
        *peak_value = peak_value_;
        out_roi->x = out_rect.x;
        out_roi->y = out_rect.y;
        out_roi->width = out_rect.width;
        out_roi->height = out_rect.height;
    }
    *dsp_usec = (static_cast<int32_t>(tic.toc()));
    FARF(HIGH, "tracker_kcf_detect, %lluus, out_roi:[%d,%d,%d,%d], peak_value:%f\n", tic.toc(), out_roi->x, out_roi->y, out_roi->width, out_roi->height, *peak_value);
    ctx->frame_cnt++;
    return ret;
}

AEEResult tracker_kcf_get_debug(remote_handle64 h, uint8 *data, int datalen) {
    tracker_kcf_context_t *ctx = (tracker_kcf_context_t*) h;
    memcpy(data, ctx->tracker->debug_buf, datalen);
    return 0;
}

AEEResult tracker_kcf_set_clocks(remote_handle64 h) {

    int err;

    // Set client class
    HAP_power_request_t request;
    memset(&request, 0, sizeof(HAP_power_request_t));
    request.type = HAP_power_set_apptype;
    request.apptype = HAP_POWER_COMPUTE_CLIENT_CLASS;
    tracker_kcf_context_t *tracker_kcf_ctx = (tracker_kcf_context_t*) h;
    if ( (err = HAP_power_set((void*) tracker_kcf_ctx, &request)) != 0 ) {
        return err;
    }

    // Set to turbo and disable DCVS
    memset(&request, 0, sizeof(HAP_power_request_t));
    request.type = HAP_power_set_DCVS_v2;
    request.dcvs_v2.dcvs_enable = FALSE;
    request.dcvs_v2.set_dcvs_params = TRUE;
    request.dcvs_v2.dcvs_params.min_corner = HAP_DCVS_VCORNER_DISABLE;
    request.dcvs_v2.dcvs_params.max_corner = HAP_DCVS_VCORNER_DISABLE;
    request.dcvs_v2.dcvs_params.target_corner = HAP_DCVS_VCORNER_TURBO;
    request.dcvs_v2.set_latency = TRUE;
    request.dcvs_v2.latency = 100;
    if ( (err = HAP_power_set((void*) tracker_kcf_ctx, &request)) != 0 ) {
        return err;
    }

    // Vote for HVX power
    memset(&request, 0, sizeof(HAP_power_request_t));
    request.type = HAP_power_set_HVX;
    request.hvx.power_up = TRUE;
    return HAP_power_set((void*) tracker_kcf_ctx, &request);
}
